function [fusion] = fcn_fusion_dekf_10s_RW_w_bias(gyro, accb, magb, stoch_p, q_initial, pos_initial, no_int, acc1g, dt)
%fcn_fusion_dekf_10s_RW_w_bias   	This function uses an extended kalman filter.

%   [fusion] = fcn_fusion_dekf_10s_RW_w_bias(gyro, accb, magb, stoch_p, q_initial, vel_initial, pos_initial, no_int, acc1g, dt)
%   EKF in direct formulation with 10 states: 
%   - q
%   - wt
%   - wb

%   A Random Walk is used to model the gyro bias.
%   A bias estimation through input noise is present.

%   IN      gyro        Gyro measurements error-prone
%   IN      accb        Acc measurements error-prone
%   IN      magb        Mag measurements error-prone
%   IN      stoch_p     Vector containing stochastic parameters for error modells
%   IN      q_initial   Initial quaternion attitude
%   IN      vel_initial Initial velocity
%   IN      pos_initial Initial position
%   IN      no_int      Number of samples before position integration starts (time for the algorithm to reach a steady state)
%   IN      acc1g       Gravity (equals the gravity at Analog Devices Calibration site)
%   IN      dt          Sample period
%   OUT     fusion      Fusioned data


%% Get QUEST initial attitude if wanted
if strcmpi(q_initial, 'QUEST')
	quest_noise = [stoch_p.Nacc; stoch_p.Nmag];
    obs = [ [accb(1,:)/norm(accb(1,:))]', [magb(1,:)/norm(magb(1,:))]'];
    ref = [[0, 0, 1]', [1, 0, 0]'];
    q_initial = quest(obs, ref, quest_noise, 'noise vectors', 1);    
end

%% For algorithm
% Get loop length
k = length(gyro);

% Save for output
% States
q_save   = zeros(k, 4);                         % Quaternion attitude
wt_save  = zeros(k, 3);                         % True gyroscope
wb_save  = zeros(k, 3);                         % Estimated gyro bias
at_save  = zeros(k, 3);                         % True acceleration
ab_save  = zeros(k, 3);                         % Estimated acc bias
vel_save = zeros(k, 3);                         % Velocity
pos_save = zeros(k, 3);                         % Position
% Kalman
F_save = zeros(k, 10, 10);                      % Jacobian of the state transition equation
P_save = zeros(k, 10, 10);                      % Error covariance matrix
% ACC update
h_acc_save = zeros(k, 3);                      	% Measurement equation of the ACC update
y_acc_save = zeros(k, 3);                      	% Errors of the ACC update
S_acc_save = zeros(k, 3, 3);                  	% Innovation covariance of the ACC update
K_acc_save = zeros(k, 10, 3);                 	% Kalman gain of the ACC update
% MAG update
h_mag_save = zeros(k, 3);                       % Measurement equation of the MAG update
y_mag_save = zeros(k, 3);                       % Errors of the MAG update
S_mag_save = zeros(k, 3, 3);                    % Innovation covariance of the MAG update
K_mag_save = zeros(k, 10, 3);               	% Kalman gain of the MAG update
% external update
h_ext_save = zeros(k, 6);                      	% Measurement equation of external velocity and position update
y_ext_save = zeros(k, 6);                      	% Errors of external velocity and position update
S_ext_save = zeros(k, 6, 6);                  	% Innovation covariance of external velocity and position update
K_ext_save = zeros(k, 22, 6);                 	% Kalman gain of external velocity and position update

% Kalman initials
% Attitude
q = q_initial;

% Gyroscope values (w_measured = w_true + w_bias)
wm = zeros(1, 3);
wt = zeros(1, 3);
wb = zeros(1, 3);

% Accelerometer values (a_measured = a_true + a_bias)
am = zeros(1, 3);
at = zeros(1, 3);
ab = zeros(1, 3);
g = [0; 0; acc1g];

% Magnetometer values
mm = zeros(1, 3);

% Velocity
v = zeros(1, 3);
vm = zeros(1, 3);

% Position
p = pos_initial; 
pm = zeros(1, 3);

%% For process noise covariance matrices
% Gyro
Q_WN_gyro = stoch_p.Ngyro^2/dt;
Q_GM_gyro = stoch_p.Bgyro^2;
Q_RW_gyro = stoch_p.Kgyro^2 * dt;
% For GM process:
Tc = (stoch_p.tc1_gyro + stoch_p.tc2_gyro) / 2;
tau = 1/Tc;
sigma = sqrt(2*log(2)/pi) * stoch_p.Bgyro;
% digital driving noise
Q_GM_DDN = (sigma^2 * (1-exp(-2*(dt/Tc))));

% Acc
Q_WN_acc = stoch_p.Nacc^2/dt;
Q_GM_acc = stoch_p.Bacc^2;
Q_RW_acc = stoch_p.Kacc^2 * dt;

% Mag
Q_WN_mag = stoch_p.Nmag^2/dt;
Q_GM_mag = stoch_p.Bmag^2;
Q_RW_mag = stoch_p.Kmag^2 * dt;


%% Initial matrices
% Initial error covariance matrix
P = zeros(10,10);  

% Process covariance matrix
Q = diag([Q_WN_gyro, Q_WN_gyro, Q_WN_gyro, Q_RW_gyro, Q_RW_gyro, Q_RW_gyro], 0);

% Measurement noise covariance matrix for ACC update
R_acc = diag([Q_WN_acc, Q_WN_acc, Q_WN_acc], 0);

% Measurement noise covariance matrix for MAG update
R_mag = diag([Q_WN_mag, Q_WN_mag, Q_WN_mag], 0);

for j=1:k,j; 
    
    %% Get quaternion matrices for easier calculation
    C = [0        -q(4)     q(3);
         q(4)     0         -q(2);
         -q(3)    q(2)      0];

    q_mat = [ q(1)                  [-q(2) -q(3) -q(4)];
            [ q(2); q(3); q(4)]       q(1)*eye(3) + C];

    q_mat_conjugate = [ q(1)                  [-q(2) -q(3) -q(4)];
                      [ q(2); q(3); q(4)]       q(1)*eye(3) - C];

    % Rotationmatrix body -> inertial
    R_mat = transpose(q_mat_conjugate)*q_mat;
    R_mat = R_mat(2:4, 2:4);

    % Rotationmatrix inertial -> body
    R_mat_conjugate = q_mat_conjugate*transpose(q_mat);      
    R_mat_conjugate = R_mat_conjugate(2:4, 2:4); 
    
    
    %% Extended Kalman Filter
    %% Prediction
    % Input
    wm = gyro(j,:);
    wt = (wm-wb)';
    theta = norm(wt*dt);
    
    % Got to pass if theta is zero! otherwise we'd end up with NaN q's
    if (theta)
    % State transition equation
    f = [   q(1)*cos(theta/2) - (dt*q(2)*wt(1)*sin(theta/2))/theta - (dt*q(3)*wt(2)*sin(theta/2))/theta - (dt*q(4)*wt(3)*sin(theta/2))/theta;
            q(2)*cos(theta/2) + (dt*q(1)*wt(1)*sin(theta/2))/theta + (dt*q(4)*wt(2)*sin(theta/2))/theta - (dt*q(3)*wt(3)*sin(theta/2))/theta;
            q(3)*cos(theta/2) - (dt*q(4)*wt(1)*sin(theta/2))/theta + (dt*q(1)*wt(2)*sin(theta/2))/theta + (dt*q(2)*wt(3)*sin(theta/2))/theta;
            q(4)*cos(theta/2) + (dt*q(3)*wt(1)*sin(theta/2))/theta - (dt*q(2)*wt(2)*sin(theta/2))/theta + (dt*q(1)*wt(3)*sin(theta/2))/theta;
                                                                                                    wm(1) - wb(1);
                                                                                                    wm(2) - wb(2);
                                                                                                    wm(3) - wb(3);
                                                                                                          wb(1);
                                                                                                          wb(2);
                                                                                                          wb(3)];
                                                                     
    % Jacobian of the state transition equation 
    F = [   cos(theta/2),                   -(dt*wt(1)*sin(theta/2))/theta,	-(dt*wt(2)*sin(theta/2))/theta	-(dt*wt(3)*sin(theta/2))/theta,	-(dt*q(2)*sin(theta/2))/theta,  -(dt*q(3)*sin(theta/2))/theta, 	-(dt*q(4)*sin(theta/2))/theta,	0,  0,  0;
            (dt*wt(1)*sin(theta/2))/theta,  cos(theta/2),                   -(dt*wt(3)*sin(theta/2))/theta,	(dt*wt(2)*sin(theta/2))/theta,  (dt*q(1)*sin(theta/2))/theta, 	(dt*q(4)*sin(theta/2))/theta, 	-(dt*q(3)*sin(theta/2))/theta,  0,  0,  0;
            (dt*wt(2)*sin(theta/2))/theta,  (dt*wt(3)*sin(theta/2))/theta,  cos(theta/2),                   -(dt*wt(1)*sin(theta/2))/theta, -(dt*q(4)*sin(theta/2))/theta, 	(dt*q(1)*sin(theta/2))/theta,  	(dt*q(2)*sin(theta/2))/theta,   0,  0,  0;
            (dt*wt(3)*sin(theta/2))/theta,  -(dt*wt(2)*sin(theta/2))/theta, (dt*wt(1)*sin(theta/2))/theta,  cos(theta/2),                   (dt*q(3)*sin(theta/2))/theta,  	-(dt*q(2)*sin(theta/2))/theta, 	(dt*q(1)*sin(theta/2))/theta,   0,  0,  0;
            0,                              0,                              0,                              0,                              0,                              0,                              0,                             -1,  0,  0;
            0,                              0,                              0,                              0,                              0,                              0,                              0,                              0, -1,  0;
            0,                              0,                              0,                              0,                              0,                              0,                              0,                              0,  0, -1;
            0,                              0,                              0,                              0,                              0,                              0,                              0,                              1,  0,  0;
            0,                              0,                              0,                              0,                              0,                              0,                              0,                              0,  1,  0;
            0,                              0,                              0,                              0,                              0,                              0,                              0,                              0,  0,  1];
    
    else
        
    f = [   q(1);
            q(2);
            q(3);
            q(4);
            wm(1) - wb(1);
            wm(2) - wb(2);
            wm(3) - wb(3);
            wb(1);
            wb(2);
            wb(3)];
        
    F = [   1, 0, 0, 0, 0, 0, 0,  0,  0,  0;
            0, 1, 0, 0, 0, 0, 0,  0,  0,  0;
            0, 0, 1, 0, 0, 0, 0,  0,  0,  0;
            0, 0, 0, 1, 0, 0, 0,  0,  0,  0;
            0, 0, 0, 0, 0, 0, 0, -1,  0,  0;
            0, 0, 0, 0, 0, 0, 0,  0, -1,  0;
            0, 0, 0, 0, 0, 0, 0,  0,  0, -1;
            0, 0, 0, 0, 0, 0, 0,  1,  0,  0;
            0, 0, 0, 0, 0, 0, 0,  0,  1,  0;
            0, 0, 0, 0, 0, 0, 0,  0,  0,  1];     
        
    end
    
	% Predict states
    x = f;
    
    % Norm quaternion after manipulation
    x(1:4) = x(1:4) / norm(x(1:4));   
    
    % Inputs
    Fu = [  0, 0, 0, 0, 0, 0;
            0, 0, 0, 0, 0, 0;
            0, 0, 0, 0, 0, 0;
            0, 0, 0, 0, 0, 0;
            1, 0, 0, 0, 0, 0;
            0, 1, 0, 0, 0, 0;
            0, 0, 1, 0, 0, 0;
            0, 0, 0, 1, 0, 0;
            0, 0, 0, 0, 1, 0;
            0, 0, 0, 0, 0, 1];

    % Predict error covariance
    P = F*P*F' + Fu*Q*Fu';
        
    
    %% Correction #1: based on ACC update
    % Measurements
    am = accb(j,:);
    
    % Normalize vectors for attitude determination
    am_n = am / norm(am); 
    
	% Measurement vector
	z_acc = [am_n]';
    
    % Measurement equation
    h_acc = [   R_mat_conjugate * [0 0 1]'];     
       
    % Jacobian of the measurement equation
    H_acc =  [  -2*q(3),                    2*q(4),                     -2*q(1),                    2*q(2),                     0, 0, 0, 0, 0, 0;
                2*q(2),                     2*q(1),                     2*q(4),                     2*q(3),                     0, 0, 0, 0, 0, 0;
                2*q(1),                     -2*q(2),                    -2*q(3),                    2*q(4),                     0, 0, 0, 0, 0, 0];
            
    % Innovation
    y_acc = z_acc - h_acc;     
    
    % Innovation covariance (residual)
    S_acc = H_acc*P*H_acc' + R_acc;
        
    % Compute Kalman gain
    K_acc = P*H_acc' * inv(S_acc);    
      
    % Update the estimate (measurement residual)
    x = x + K_acc * y_acc;
    
    % Norm quaternion after manipulation
    x(1:4) = x(1:4) / norm(x(1:4)); 
    
    % Update error covariance
    P = (eye(10) - K_acc*H_acc) * P;
    
    
    %% Correction #2: based on MAG update
    % Measurements
    mm = magb(j,:);  
    
    % Normalize vectors for attitude determination
    mm_n = mm / norm(mm);    
    
	% Measurement vector
	z_mag = [mm_n]';
    
    % Measurement equation
    % Madgwick Magnetic Distortion
    mm_ni = R_mat * mm_n';                                      % body -> inertial
    b = [sqrt(mm_ni(1)^2 + mm_ni(2)^2); 0; mm_ni(3)];           % turn vector north
       
    h_mag = [   R_mat_conjugate * b];     
       
    % Jacobian of the measurement equation
    H_mag =  [  2*b(1)*q(1) - 2*b(3)*q(3),  2*b(1)*q(2) + 2*b(3)*q(4), -2*b(1)*q(3) - 2*b(3)*q(1),  2*b(3)*q(2) - 2*b(1)*q(4),  0, 0, 0, 0, 0, 0;
                2*b(3)*q(2) - 2*b(1)*q(4),  2*b(1)*q(3) + 2*b(3)*q(1), 	2*b(1)*q(2) + 2*b(3)*q(4),  2*b(3)*q(3) - 2*b(1)*q(1),  0, 0, 0, 0, 0, 0;
                2*b(1)*q(3) + 2*b(3)*q(1),  2*b(1)*q(4) - 2*b(3)*q(2),	2*b(1)*q(1) - 2*b(3)*q(3),  2*b(1)*q(2) + 2*b(3)*q(4),  0, 0, 0, 0, 0, 0]; 
        
	% Innovation
    y_mag = z_mag - h_mag;            
    
    % Innovation covariance (residual)
    S_mag = H_mag*P*H_mag' + R_mag;
        
    % Compute Kalman gain
    K_mag = P*H_mag' * inv(S_mag);    
      
    % Update the estimate (measurement residual)
    x = x + K_mag * y_mag;
    
    % Norm quaternion after manipulation
    x(1:4) = x(1:4) / norm(x(1:4)); 
    
    % Update error covariance
    P = (eye(10) - K_mag*H_mag) * P;
    
    % Extract states from state vector
    q = x(1:4)';
	wt = x(5:7)';
    wb = x(8:10)';
    
    % Save states
	q_save(j,:)  = q;
    wt_save(j,:) = wt;
    wb_save(j,:) = wb;
    
    % Save Kalman variables
    F_save(j,:,:) = F;
    P_save(j,:,:) = P;
    h_acc_save(j,:) = h_acc';
    S_acc_save(j,:,:) = S_acc;
    y_acc_save(j,:) = y_acc';
    K_acc_save(j,:,:) = K_acc;
    h_mag_save(j,:) = h_mag';
    S_mag_save(j,:,:) = S_mag;
    y_mag_save(j,:) = y_mag';
    K_mag_save(j,:,:) = K_mag;    
    
    % ONLY start position integration if ss_time is over (algorithm and
    % attitude reached steady state)
    if (j > no_int)
        % Velocity and Position
        % Transfer acc vector to NED (inertial) frame
        acc_earth = quatmultiply((q), quatmultiply([0 am], quatconj(q)));
        acc_earth = acc_earth(1, 2:4);

        % Substract 1g from z-component
        acc_earth(3) = acc_earth(3) - acc1g;

        % Integrate to velocity
        v = v + acc_earth * dt;
        
        % Save for output
        vel_save(j,:) = v;

        % Integrate to position
        p = p + v * dt;

        % Save for output
        pos_save(j,:) = p;
    
    else
        % Save for output        
        vel_save(j,:) = v;
        pos_save(j,:) = p;
   
    end

end


%% Output
% Eulerangles from quaternion attitude
[yaw, pitch, roll] = quat2angle( q_save, 'ZYX' );

% Transform vectors to inertial frame
tmp(:,2:4) = accb;
acci_save = quatmultiply(q_save, quatmultiply(tmp, quatconj(q_save)));
tmp(:,2:4) = magb;
magi_save = quatmultiply(q_save, quatmultiply(tmp, quatconj(q_save)));

% Output sample time
for j=1:k,j;
    fusion.time(j,1) = j*dt;
end

% Output position
fusion.X = pos_save(:,1);
fusion.Y = pos_save(:,2);
fusion.Z = pos_save(:,3);
fusion.pos = pos_save;

% Output velocity
fusion.vel = vel_save;
fusion.v_xy = (vel_save(:,1).^2 + vel_save(:,2).^2).^(1/2);

% Output acceleration
fusion.a_xy = (acci_save(:,2).^2 + acci_save(:,3).^2).^(1/2);

% Output euler angles
fusion.phi = roll;
fusion.theta = pitch;
fusion.psi = yaw;

% Output quaternion attitude
fusion.q = q_save;
% Output quaternion attitude where position integration starts
fusion.q_int = q_save(no_int+1,:);

% Output rest of states
fusion.wt = wt_save;
fusion.wb = wb_save;

% Output Kalman variables
fusion.F = F_save;
fusion.P = P_save;
fusion.h_acc = h_acc_save;
fusion.y_acc = y_acc_save;
fusion.S_acc = S_acc_save;
fusion.K_acc = K_acc_save;
fusion.h_mag = h_mag_save;
fusion.y_mag = y_mag_save;
fusion.S_mag = S_mag_save;
fusion.K_mag = K_mag_save;

% Output direction vectors: acceleration
fusion.acci = acci_save(:,2:4);

% Output direction vectors: magnetometer
fusion.magi = magi_save(:,2:4);

end

